#include "sigmundcpp.h"

sigmundcpp::sigmundcpp(int npts, 
                       int hop, 
                       int npeak, 
                       float maxfreq, 
                       float vibrato, 
                       float stabletime, 
                       float growth, 
                       float minpower)
: m_npts(NPOINTS_DEF)
, m_param1(6)
, m_param2(0.5)
, m_param3(0)
, m_mode(MODE_STREAM)
, m_stabletime(STABLETIME_DEF)
, m_growth(GROWTH_DEF)
, m_minpower(MINPOWER_DEF)
, m_maxfreq(1000000)
, m_loud(0)
, m_sr(44100.)
, m_trackv(0)
, m_ntrack(0)
//, m_dopitch(1)
//, m_donote(1)
//, m_dotracks(1)
, m_inbuf(0)
, m_lastnote(0)
, m_lastfreq(0)
, m_lastenv(0)
{
  m_infill = m_countdown = 0;
  set_hop(hop);
  set_npeak(npeak);
  set_vibrato(vibrato);
  set_stabletime(stabletime);
  set_growth(growth);
  set_minpower(minpower);

  m_ntrack = m_npeak;
  m_trackv = new t_peak[m_ntrack];
  
  set_npts(m_npts);
  notefinder_init(&m_notefinder);
}

sigmundcpp::~sigmundcpp()
{
  if (m_inbuf)
    delete [] m_inbuf;
  
  if (m_trackv)
    delete [] m_trackv;
}

void sigmundcpp::set_sr(float v)
{
  m_sr = (int) v;
}

void sigmundcpp::set_hop(int v)
{
  m_hop = v;
  
  // check parameter ranges
  
  if (m_hop != (1 << sigmund_ilog2(m_hop)))
    printf("sigmund~: adjusting analysis size to %d points", (m_hop = (1 << sigmund_ilog2(m_hop))));
}

void sigmundcpp::set_npeak(int v)
{
  if (v < 1)
    v = 1;
  
  m_npeak = v;
}

void sigmundcpp::set_vibrato(float v)
{
  if (v < 0.)
    v = 0.;
  
  m_vibrato = v;
}

void sigmundcpp::set_stabletime(float v)
{
  if (v < 0.)
    v = 0.;
  
  m_stabletime = v;
}

void sigmundcpp::set_growth(float v)
{
  if (v < 0.)
    v = 0.;
  
  m_growth = v;
}

void sigmundcpp::set_minpower(float v)
{
  if (v < 0.)
    v = 0.;
  
  m_minpower = v;
}

void sigmundcpp::set_npts(int v)
{
  int nwas = m_npts, npts = v;
  
  // check parameter ranges
  
  if (npts < NPOINTS_MIN)
    printf("sigmund~: minimum points %d", NPOINTS_MIN),
    npts = NPOINTS_MIN;
  
  if (npts != (1 << sigmund_ilog2(npts)))
    printf("sigmund~: adjusting analysis size to %d points",
         (npts = (1 << sigmund_ilog2(npts))));
  
  if (npts != nwas)
    m_countdown = m_infill  = 0;
  
  if (m_mode == MODE_STREAM)
  {
    if (m_inbuf)
      delete [] m_inbuf;

    m_inbuf = new t_sample[npts];
    memset((m_inbuf), 0, sizeof(t_sample) * npts);
  }
  else m_inbuf = 0;
  m_npts = npts;
}

void sigmundcpp::clear()
{
  if (m_trackv)
    memset(m_trackv, 0, m_ntrack * sizeof(t_peak));
  
  m_infill = m_countdown = 0;
}

void sigmundcpp::processBlock(float* input, int nSamples)
{
  if (m_hop % nSamples)
    return;
  
  if (m_countdown > 0)
    m_countdown -= nSamples;
  
  else if (m_infill != m_npts)
  {
    float *fp = m_inbuf + m_infill;
    
    for (int j = 0; j < nSamples; j++)
      *fp++ = *input++;
    
    m_infill += nSamples;
    
    if (m_infill == m_npts)
      bufferFull();
  }
}

void sigmundcpp::bufferFull()
{
  if (m_infill == m_npts)
  {
    calc(m_npts, m_inbuf, m_loud, m_sr);
    
    if (m_hop >= m_npts)
    {
      m_infill = 0;
      m_countdown = m_hop - m_npts;
    }
    else
    {
      memmove(m_inbuf, m_inbuf + m_hop,
              (m_infill = m_npts - m_hop) * sizeof(*m_inbuf));
      
      m_countdown = 0;
    }
    
    if (m_loud)
      m_loud--;
  }
}

void sigmundcpp::calc(int npts, float *arraypoints, int loud, float srate)
{
  t_peak* peakv = (t_peak*) alloca(m_npeak * sizeof(t_peak));
  
  int nfound;
  float freq = 0, power, note = 0;
  sigmund_getrawpeaks(npts, arraypoints, m_npeak, peakv,
                      &nfound, &power, srate, loud, m_maxfreq);
  //if (m_dopitch)
    sigmund_getpitch(nfound, peakv, &freq, npts, srate, 
                     m_param1, m_param2, loud);
  //if (m_donote)
    notefinder_doit(&m_notefinder, freq, power, &note, m_vibrato, 
                    1 + m_stabletime * 0.001f * m_sr / (float)m_hop,
                    exp(LOG10*0.1*(m_minpower - 100)), m_growth, loud);
  //if (m_dotracks)
    sigmund_peaktrack(nfound, peakv, m_ntrack, m_trackv, loud);
  
  m_lastfreq = freq;
  m_lastenv = power;
  m_lastnote = note;

//  for (int i = 0; i < nfound; i++)
//  {
//    printf("peaks %i %f %f %f %f\n", i, peakv[i].p_freq, 2*peakv[i].p_amp, 2*peakv[i].p_ampreal, 2*peakv[i].p_ampimag);
//  }
//  
//  for (int i = 0; i < m_ntrack; i++)
//  {
//    printf("track %i %f %f %f\n", i, m_trackv[i].p_freq, 2*m_trackv[i].p_amp, m_trackv[i].p_tmp);
//  }
}

float sigmundcpp::getFreq()
{
  return m_lastfreq;
}

float sigmundcpp::getPitch()
{
  return sigmund_ftom(m_lastfreq);
}

float sigmundcpp::getNote()
{
  return sigmund_ftom(m_lastnote);
}

float sigmundcpp::getEnv()
{
  return sigmund_powtodb(m_lastenv);
}
